#ifdef WIN32
#define _USE_MATH_DEFINES
#endif
#include "Quaternion.h"
#include <iostream>

Quaternion::Quaternion()
{
    m_i = m_j = m_k = 0;
    m_r = 1;
}

Quaternion::Quaternion(const float r, const float i, const float j, const float k) :
m_r(r),
m_i(i),
m_j(j),
m_k(k)
{}

Quaternion::Quaternion(const Quaternion& q) :
m_r(q.m_r),
m_i(q.m_i),
m_j(q.m_j),
m_k(q.m_k)
{}

Quaternion::Quaternion(const Vec3& axis, float degrees)
{
    float angle = float((degrees / 180.0f) * M_PI);
    float sinA = sin(angle /2);
    Vec3 ax(axis);
    ax.normalise();
    m_r = cos(angle / 2);
    m_i = ax.X() * sinA;
    m_j = ax.Y() * sinA;
    m_k = ax.Z() * sinA;
}

Quaternion& Quaternion::Normalise()
{
    float length = (m_r*m_r) + (m_i*m_i) + (m_j*m_j) + (m_k*m_k);
    if (length == 0) 
    {
        m_r = 1;
        return *this;
    }
    if(length > 1.0000001)
    {
        length = 1.0f / sqrt(length);
        m_r *= length;
        m_i *= length;
        m_j *= length;
        m_k *= length;
    }
    return *this;
}

Quaternion& Quaternion::operator*=(const Quaternion& other)
{
    Quaternion q = *this;
    m_r = (q.m_r * other.m_r) - (q.m_i * other.m_i)-
          (q.m_j * other.m_j) - (q.m_k * other.m_k);
    m_i = (q.m_r * other.m_i) + (q.m_i * other.m_r)+ 
          (q.m_j * other.m_k) - (q.m_k * other.m_j);
    m_j = (q.m_r * other.m_j) + (q.m_j * other.m_r)+ 
          (q.m_k * other.m_i) - (q.m_i * other.m_k);
    m_k = (q.m_r * other.m_k) + (q.m_k * other.m_r) + 
          (q.m_i * other.m_j) - (q.m_j * other.m_i);

    return *this;
}
/* This is for angular velocity only :) */
Quaternion& Quaternion::AddScaleVector(const Vec3& v, float scale)
{
    Quaternion q(0, 
        v.X() * scale, 
        v.Y() * scale, 
        v.Z() * scale);
    q *= *this;
    m_r += q.m_r * 0.5f;
    m_i += q.m_i * 0.5f;
    m_j += q.m_j * 0.5f;
    m_k += q.m_k * 0.5f;
    return *this;
}

/* Again, angular velocity only, not an every day function */
Quaternion& Quaternion::RotateByVector(const Vec3& v)
{
    Quaternion q(0, v.X(),v.Y(),v.Z());
    (*this) *= q;
    return *this;
}

Quaternion& Quaternion::SetStats(const Vec3& axis, float degrees)
{
    float angle = float((degrees / 180.0f) * M_PI);
    float sinA = sin(angle /2);
    Vec3 ax(axis);
    ax.normalise();
    m_r = cos(angle / 2);
    m_i = ax.X() * sinA;
    m_j = ax.Y() * sinA;
    m_k = ax.Z() * sinA;
    return *this;
}
Quaternion& Quaternion::SetStats(const float i, float j, float k, float degrees)
{
    float angle = float((degrees / 180.0f) * M_PI);
    float sinA = sin(angle /2);
    Vec3 ax(i,j,k);
    ax.normalise();
    m_r = cos(angle / 2);
    m_i = ax.X() * sinA;
    m_j = ax.Y() * sinA;
    m_k = ax.Z() * sinA;
    return *this;
}

void Quaternion::Print() const
{
    std::cout << " m_r : " << m_r << std::endl;
    std::cout << " m_i : " << m_i << std::endl;
    std::cout << " m_j : " << m_j << std::endl;
    std::cout << " m_k : " << m_k << std::endl;
}
